#!/usr/bin/env python
# -*- coding: utf-8 -*-

#combination of cf and other information

import rospy
from std_msgs.msg import String
from nav_msgs.msg import Path
from pycrazyswarm import Crazyswarm
import numpy as np				
import math


TAKEOFF_DURATION = 2.5
HOVER_DURATION = 5.0

#formation_ex=String()
#formation_ex.data='NULL'
Z=0.5
Y=0.3

kPosition = 0.5



def UP():
	flag=1
	for cf in allcfs.crazyflies:
		if cf.position()[2]<1.3:
			pos = cf.position() + np.array([0, 0, Z])
			cf.goTo(pos,0,1)
			print('sending')
		else :
			flag=0
			break
	timeHelper.sleep(1.5)
	if flag==1:
		print("Already UP")
	else :
		print("Alread HIGH ENOUGH")


def DOWN():
	flag=1
	for cf in allcfs.crazyflies:
		print(cf.position()[2])
		if cf.position()[2]>0.55:
			pos = cf.position() - np.array([0, 0, Z])
			cf.goTo(pos,0,1)
		else :
			flag=0
			break
	timeHelper.sleep(1.5)
	if flag==1:
		print("Already DOWN")
		return True
	else :
		print("READY TO LAND")
		allcfs.land(targetHeight=0.04, duration=1.5)
		return False

def RIGHT():
	for cf in allcfs.crazyflies:
		pos = cf.position() - np.array([0, Y, 0])
		cf.goTo(pos,0,1)
	timeHelper.sleep(1.5)

def LEFT():
	for cf in allcfs.crazyflies:
		pos = cf.position() + np.array([0, Y, 0])
		cf.goTo(pos,0,1)
	timeHelper.sleep(1.5)

def CCW():
	totalTime = 10
	r_leader=[0,0,0]
	# the position of the leader
	vr_leader=[0,0,0.4/totalTime]
	# the velocity of the leader
	for cf in allcfs.crazyflies:
		pos = cf.position()
		cf.cmdPosition(pos)
		r_leader+=0.25*pos
	timeHelper.sleep(0.5)

	radius=[0,0,0,0]
	omega0=[0,0,0,0]
	for i,cf in enumerate(allcfs.crazyflies):
		p1=np.array([cf.position()[0],cf.position()[1]])
		print(p1)
		p2=np.array([r_leader[0],r_leader[1]])
		p3=p2-p1
		radius[i]=math.hypot(p3[0],p3[1])
		omega = 2 * np.pi / totalTime
		omega0[i]= math.atan2(cf.position()[1]-r_leader[1],cf.position()[0]-r_leader[0])
		timeStage = timeHelper.time()
	print(radius)
	while True:
		time = timeHelper.time() - timeStage
		if time>totalTime:
			for i,cf in enumerate(allcfs.crazyflies):
				cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
				timeHelper.sleepForRate(30)
			for i,cf in enumerate(allcfs.crazyflies):
				cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
				timeHelper.sleepForRate(30)
			print('this circle is over')
			break
		for i,cf in enumerate(allcfs.crazyflies):
			vx = -radius[i] * omega * np.sin(omega * time + omega0[i])
			vy = radius[i] * omega * np.cos(omega * time + omega0[i])
			## Y : the accurate one 
			desiredPos = r_leader + radius[i] * np.array(
                    [np.cos(omega * time + omega0[i]), np.sin(omega * time + omega0[i]), 0])
			errorX = desiredPos - cf.position()
			## Y : the P(ID)
			cf.cmdVelocityWorld(np.array([vx, vy, 0] + kPosition* errorX + np.array(vr_leader)), yawRate=0)
			timeHelper.sleepForRate(30)
	cf.notifySetpointsStop()
	for i,cf in enumerate(allcfs.crazyflies):
		pos = cf.position() 
		cf.goTo(pos,0,1)
	timeHelper.sleep(1.5)
	print ('READY FOR INSTRUCTIONS')
			

def CW():
	totalTime = 10
	r_leader=[0,0,0]
	# the position of the leader
	vr_leader=[0,0,-0.4/totalTime]
	# the velocity of the leader
	for cf in allcfs.crazyflies:
		pos = cf.position()
		cf.cmdPosition(pos)
		r_leader+=0.25*pos
	timeHelper.sleep(0.5)

	radius=[0,0,0,0]
	omega0=[0,0,0,0]
	for i,cf in enumerate(allcfs.crazyflies):
		p1=np.array([cf.position()[0],cf.position()[1]])
		print(p1)
		p2=np.array([r_leader[0],r_leader[1]])
		p3=p2-p1
		radius[i]=math.hypot(p3[0],p3[1])
		omega = 2 * np.pi / totalTime
		omega0[i]= math.atan2(cf.position()[1]-r_leader[1],cf.position()[0]-r_leader[0])
	print(radius)
	timeStage = timeHelper.time()
	while True:
		time = timeHelper.time() - timeStage
		if time>totalTime:
			for i,cf in enumerate(allcfs.crazyflies):
				cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
				timeHelper.sleepForRate(30)
			for i,cf in enumerate(allcfs.crazyflies):
				cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
				timeHelper.sleepForRate(30)
			print('this circle is over')
			break
		for i,cf in enumerate(allcfs.crazyflies):
			vx = +radius[i] * omega * np.sin(-omega * time + omega0[i])
			vy = -radius[i] * omega * np.cos(-omega * time + omega0[i])
			desiredPos = r_leader + radius[i] * np.array(
                    [np.cos(-omega * time + omega0[i]), np.sin(-omega * time + omega0[i]), 0])
			## the leader plus the ohters
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld(np.array([vx, vy, 0] + kPosition* errorX + np.array(vr_leader)), yawRate=0)
			timeHelper.sleepForRate(30)
	cf.notifySetpointsStop()
	for i,cf in enumerate(allcfs.crazyflies):
		pos = cf.position() 
		cf.goTo(pos,0,1)
	print ('READY FOR INSTRUCTIONS')

def F2():
	totalTime = 10
	r_leader=[0,0,0]
	# the position of the leader
	for cf in allcfs.crazyflies:
		pos = cf.position()
		cf.cmdPosition(pos)
		r_leader+=0.25*pos
	timeHelper.sleep(0.5)
	timeStage4 = timeHelper.time()
	while True:
		time = timeHelper.time() - timeStage4
		if time > totalTime	/2 :
			break
		vr_leader  = [0 / totalTime, 0 / totalTime, 0]
		r_leader += np.array([0 / totalTime*time ,0/ totalTime*time , 0])
		for i,cf in enumerate(allcfs.crazyflies):
			omega0 = -i * 2 * np.pi / len(allcfs.crazyflies) + np.pi / 4
			radius = 0.5
			desiredPos = r_leader + radius *np.array([np.cos(omega0), np.sin(omega0), 0])
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld( kPosition * errorX + np.array(vr_leader), yawRate=0)
			timeHelper.sleepForRate(30)	
	cf.notifySetpointsStop()
	for i,cf in enumerate(allcfs.crazyflies):
		cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
		timeHelper.sleepForRate(30)
	for i,cf in enumerate(allcfs.crazyflies):
		pos = cf.position() 
		cf.goTo(pos,0,1)
	print ('READY FOR INSTRUCTIONS')
	
def F1():
	totalTime = 10
	r_leader=[0,0,0]
	# the position of the leader
	for cf in allcfs.crazyflies:
		pos = cf.position()
		cf.cmdPosition(pos)
		r_leader+=0.25*pos
	timeHelper.sleep(0.5)
	timeStage = timeHelper.time()
	while True:
		time = timeHelper.time() - timeStage
		if time > totalTime	/2 :
			break
		vr_leader  = [0, 0, 0]
		for i,cf in enumerate(allcfs.crazyflies):
			x_i= 0.9 -i *0.6
			desiredPos = r_leader + np.array([x_i, 0, 0])
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld( kPosition * errorX + np.array(vr_leader) , yawRate=0)
			timeHelper.sleepForRate(30)	
	timeStage2 = timeHelper.time()
	while True:
		time = timeHelper.time() - timeStage2
		if time > totalTime	/2 :
			break
		for i,cf in enumerate(allcfs.crazyflies):
			x_i= 0.9 -i *0.6
			desiredPos = r_leader + np.array([0 , x_i , 0])
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld( kPosition * errorX + np.array(vr_leader) , yawRate=0)
			timeHelper.sleepForRate(30)
	timeStage3 = timeHelper.time()
	while True:
		time = timeHelper.time() - timeStage3
		if time > totalTime	/2 :
			break
		vr_leader  = [0, 0, 0]
		for i,cf in enumerate(allcfs.crazyflies):
			x_i= 0.9 -i *0.6
			desiredPos = r_leader + np.array([x_i, 0, 0])
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld( kPosition * errorX + np.array(vr_leader) , yawRate=0)
			timeHelper.sleepForRate(30)	
	
	cf.notifySetpointsStop()
	for i,cf in enumerate(allcfs.crazyflies):
		cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
		timeHelper.sleepForRate(30)
	for i,cf in enumerate(allcfs.crazyflies):
		pos = cf.position() 
		cf.goTo(pos,0,1)
	print ('READY FOR INSTRUCTIONS')


def F4():
	totalTime = 10
	r_leader=[0,0,0]
	# the position of the leader
	for cf in allcfs.crazyflies:
		pos = cf.position()
		cf.cmdPosition(pos)
		r_leader+=0.25*pos
	timeHelper.sleep(0.5)
	timeStage14 = timeHelper.time()
	h = np.array([[0.8, 0 ,0],[0, 0, 0],[0.8*np.cos(np.pi * 7 / 6), 0.8*np.sin(np.pi * 7 / 6), 0],[0.8*np.cos(np.pi * 5 / 6), 0.8*np.sin(np.pi * 5 / 6), 0]])

	while True:
		time = timeHelper.time() - timeStage14
		if time >  totalTime/2 :
			break
		vr = [0, 0, 0]
		for i,cf in enumerate(allcfs.crazyflies):
			omega = 0
			vx = 0
			vy = 0
			vz = 0
			desiredPos = r_leader +  h[i,...]
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld( kPosition * errorX + np.array(vr), yawRate=0)
			timeHelper.sleepForRate(30)
	cf.notifySetpointsStop()
	for i,cf in enumerate(allcfs.crazyflies):
		cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
		timeHelper.sleepForRate(30)
	for i,cf in enumerate(allcfs.crazyflies):
		pos = cf.position() 
		cf.goTo(pos,0,1)
	print ('READY FOR INSTRUCTIONS')

def F3():
	totalTime = 10
	r_leader=[0,0,0]
	# the position of the leader
	for cf in allcfs.crazyflies:
		pos = cf.position()
		cf.cmdPosition(pos)
		r_leader+=0.25*pos
	timeHelper.sleep(0.5)
	timeStage14 = timeHelper.time()
	radius = 0.4
	h = np.array([[radius, 0 ,0],[-radius, 0, 0],[-radius, -2*radius, 0], [-radius, 2*radius, 0]])

	while True:
		time = timeHelper.time() - timeStage14
		if time >  totalTime/2 :
			break
		vr = [0, 0, 0]
		for i,cf in enumerate(allcfs.crazyflies):
			omega = 0
			vx = 0
			vy = 0
			vz = 0
			desiredPos = r_leader +  h[i,...]
			errorX = desiredPos - cf.position()
			cf.cmdVelocityWorld( kPosition * errorX + np.array(vr), yawRate=0)
			timeHelper.sleepForRate(30)
	cf.notifySetpointsStop()
	for i,cf in enumerate(allcfs.crazyflies):
		cf.cmdVelocityWorld(np.array([0, 0, 0]), yawRate=0)
		timeHelper.sleepForRate(30)
	for i,cf in enumerate(allcfs.crazyflies):
		pos = cf.position() 
		cf.goTo(pos,0,1)
	print ('READY FOR INSTRUCTIONS')

if __name__ == '__main__':
	swarm = Crazyswarm()
	timeHelper = swarm.timeHelper
	allcfs = swarm.allcfs
	print('The number of uavs is ' + str(len(allcfs.crazyflies)))
	allcfs.takeoff(targetHeight=Z, duration=1.5+Z)
	timeHelper.sleep(2 + Z)
	formation = String()
	PATH_INS  = Path()
	FLG=0
	#rospy.init_node('subscribe_string')
	while True:
		rospy.sleep(2)
		formation = rospy.wait_for_message("/HMI/cmd", String, timeout=None)
		print(formation)
		if formation.data=='UP':
			UP()
		elif formation.data=='DOWN':
			endup=DOWN()
			if endup == False :
				break
		elif formation.data=='RIGHT':
			RIGHT()	
		elif formation.data=='LEFT':
			LEFT()	
		elif formation.data=='CW':
			CW()
		elif formation.data=='CCW':
			CCW()
		elif formation.data=='F2':
			F2()
		elif formation.data=='F1':
			F1()
		elif formation.data=='F3':
			F3()
		elif formation.data=='F4':
			F4()
		else:
			print("NO RECEIVED")
	
		
	#	PATH_INS = rospy.wait_for_message("/HMI/Path_setpoint", Path, timeout=None)
	#	x=PATH_INS.poses[0].pose.position.x
	#	y=PATH_INS.poses[0].pose.position.y
	#	z=PATH_INS.poses[0].pose.position.z
	#	formation.data='GG'
	
